/*
    Copyright (c) 2011 Marcus Schwartz.  All rights reserved.
 
    This file is part of NetObjects-Arduino.

    NetObjects-Arduino is free software: you can redistribute it and/or 
    modify it under the terms of the GNU Lesser General Public License as 
    published by the Free Software Foundation, either version 3 of the 
    License, or (at your option) any later version.

    NetObjects-Arduino is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public 
    License along with NetObjects-Arduino.  If not, see 
    <http://www.gnu.org/licenses/>.
 */

#include <NetObjects.h>
#include <LiquidCrystal.h>
#include <XBee.h>

#define PIN_LCD_RS 2
#define PIN_LCD_RW 4
#define PIN_LCD_EN 5
#define PIN_LCD_B4 6
#define PIN_LCD_B5 7
#define PIN_LCD_B6 8
#define PIN_LCD_B7 3

#define LCD_ROWS 2
#define LCD_COLUMNS 40

#define XBEE_BAUD 19200

NetObjectsControl net;
TwoButtonJoystick joystick(&net);
SkidSteerVehicle vehicle(&net);
LiquidCrystal lcd(PIN_LCD_RS, PIN_LCD_RW, PIN_LCD_EN,
                  PIN_LCD_B4, PIN_LCD_B5, PIN_LCD_B6, PIN_LCD_B7);

void setup() {
  net.localDebug(1);
  net.begin(XBEE_BAUD);
  lcd.begin(LCD_COLUMNS, LCD_ROWS);
  delay(200);
  net.debug("DspSt");
}

void loop() {
  delay(50);
  NetObjectsUpdate update = net.getUpdate();
  switch (update.type()) {
    case MOTION_SKID_STEER_VEHICLE:
      vehicle = update.skidSteerVehicle();
      break;
    case MOTION_TWO_BUTTON_JOYSTICK:
      joystick = update.twoButtonJoystick();
      break;
  }
  
  updateLcd(net.getDebug());
}

void updateLcd(char *debug) {
  char dashboard[41];
  
  char dir = 'M';
  char turn = 'A';
  char button_c = 'c';
  char button_z = 'z';
  
  uint8_t flags = joystick.flags();
  
  if (flags & MOTION_FWD) dir = '^';
  else if (flags & MOTION_REV) dir = 'v';
  if (flags & ANGLE_LEFT) turn = '<';
  else if (flags & ANGLE_RIGHT) turn = '>';
  
  if (flags & BUTTON_C)
    button_c = 'C';
  if (flags & BUTTON_Z)
    button_z = 'Z';
    
  sprintf(dashboard, "%c%03d%c%03d %c%c %03d | L%04d R%04d H%04d %03d",
    dir, joystick.magnitude(),
    turn, joystick.angle(),
    button_c, button_z,
    net.getRssi(),
    vehicle.speed_l(),
    vehicle.speed_r(),
    vehicle.heading(),
    vehicle.rssi()
  );

  lcd.setCursor(0,0);
  lcd.print(dashboard); 
  lcd.setCursor(0,1);
  lcd.print(debug);
}
